/*
 * Copyright 2016 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#ifndef SLAM_ROS_SLAM_ROS_NODE_H
#define SLAM_ROS_SLAM_ROS_NODE_H

#include <atomic>
#include <deque>
#include <map>
#include <memory>
#include <set>
#include <string>
#include <thread>
#include <unordered_map>
#include <unordered_set>
#include <vector>

#include "absl/synchronization/mutex.h"
#include "agv_msgs/msg/agv_mode.hpp"
#include "agv_msgs/msg/agv_event.hpp"
#include "agv_msgs/msg/confidence_level.hpp"
#include "agv_msgs/msg/odometry_lite.hpp"
#include "agv_msgs/msg/pose_with_confidence.hpp"
#include "geometry_msgs/msg/point_stamped.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "sensor_msgs/msg/multi_echo_laser_scan.hpp"
#include "sensor_msgs/msg/nav_sat_fix.hpp"
#include "sensor_msgs/msg/point_cloud.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "slam/common/fixed_ratio_sampler.h"
#include "slam/mapping/map_builder_interface.h"
#include "slam/mapping/pose_extrapolator.h"
#include "slam/transform/rigid_transform.h"
#include "slam_ros/map_builder_bridge.h"
#include "slam_ros/metrics/family_factory.h"
#include "slam_ros/msg_conversion.h"
#include "slam_ros/node_constants.h"
#include "slam_ros/node_options.h"
#include "slam_ros/sensor_data_cache.h"
#include "slam_ros/trajectory_options.h"
#include "slam_ros_msgs/msg/landmark_list_pose.hpp"
#include "slam_ros_msgs/msg/mapdata_progress.hpp"
#include "slam_ros_msgs/msg/pose_with_confidence.hpp"
#include "slam_ros_msgs/msg/qr_code.hpp"
#include "slam_ros_msgs/msg/reflector_landmark.hpp"
#include "slam_ros_msgs/msg/robot_status.hpp"
#include "slam_ros_msgs/msg/status_response.hpp"
#include "slam_ros_msgs/msg/submap_entry.hpp"
#include "slam_ros_msgs/msg/submap_list.hpp"
#include "slam_ros_msgs/msg/trajectory_states.hpp"
#include "slam_ros_msgs/srv/control_sam.hpp"
#include "slam_ros_msgs/srv/finish_trajectory.hpp"
#include "slam_ros_msgs/srv/get_landmark_lists.hpp"
#include "slam_ros_msgs/srv/get_trajectory_states.hpp"
#include "slam_ros_msgs/srv/log_slam_data.hpp"
#include "slam_ros_msgs/srv/only_use_odom_predict_pose.hpp"
#include "slam_ros_msgs/srv/pgm_to_localization_map.hpp"
#include "slam_ros_msgs/srv/read_metrics.hpp"
#include "slam_ros_msgs/srv/start_trajectory.hpp"
#include "slam_ros_msgs/srv/submap_query.hpp"
#include "slam_ros_msgs/srv/trajectory_query.hpp"
#include "slam_ros_msgs/srv/write_state.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_broadcaster.h"
#include "visualization_msgs/msg/marker_array.hpp"
#include "yaml-cpp/yaml.h"
#include <json/json.h>

namespace slam_ros {

using ::slam::transform::Rigid3d;

class Node : public rclcpp::Node {
 public:
  Node(const NodeOptions& node_options,
       std::unique_ptr<slam::mapping::MapBuilderInterface> map_builder,
       tf2_ros::Buffer* tf_buffer, bool collect_metrics);
  ~Node();

  Node(const Node&) = delete;
  Node& operator=(const Node&) = delete;

  void FinishAllTrajectories();
  bool FinishTrajectory(int trajectory_id);

  void RunFinalOptimization();

  void StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options);

  std::vector<std::set<::slam::mapping::TrajectoryBuilderInterface::SensorId>>
  ComputeDefaultSensorIdsForMultipleBags(
      const std::vector<TrajectoryOptions>& bags_options) const;

  int AddOfflineTrajectory(
      const std::set<slam::mapping::TrajectoryBuilderInterface::SensorId>&
          expected_sensor_ids,
      const TrajectoryOptions& options);

  void HandleOdometryMessage(
      const int trajectory_id, const std::string& sensor_id,
      const agv_msgs::msg::OdometryLite::ConstSharedPtr msg);

  void HandleNavSatFixMessage(
      const int trajectory_id, const std::string& sensor_id,
      const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg);

  void HandleLandmarkMessage(
      const int trajectory_id, const std::string& sensor_id,
      const slam_ros_msgs::msg::LandmarkList::ConstSharedPtr msg);

  void HandleImuMessage(
      const int trajectory_id, const std::string& sensor_id,
      const sensor_msgs::msg::Imu::ConstSharedPtr msg);

  void HandleLaserScanMessage(
      const int trajectory_id, const std::string& sensor_id,
      const sensor_msgs::msg::LaserScan::ConstSharedPtr msg);

  void HandleMultiEchoLaserScanMessage(
      const int trajectory_id, const std::string& sensor_id,
      const sensor_msgs::msg::MultiEchoLaserScan::ConstSharedPtr msg);

  void HandlePointCloud2Message(
      const int trajectory_id, const std::string& sensor_id,
      const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg);
  
  void HandleQRCodeMessage(
      const int trajectory_id, const std::string& sensor_id,
      const slam_ros_msgs::msg::QRCode::ConstSharedPtr msg);

  void ProcessLogSensorData();

  void SerializeState(const std::string& filename, bool cover_old_map,
                      bool include_unfinished_submaps);

  void PbstreamToPgm(const std::string& pbstream_filename,
                     const std::string& map_filename, double resolution) const;

  void LoadState(const std::string& state_filename, bool load_frozen_state);

  void PublishPose(::slam::common::Time time, Rigid3d T_map_odom,
                   Rigid3d odom_pose,
                   ::slam::mapping::ReflectorMatchResult reflector_match_result);

  void SetSlamType(SlamType type);
  void SetLasetToBaseTransform(const Rigid3d& T_laser_base);

  Rigid3d map_init_pose_from_yaml;
  bool loadRTYAML(const std::string& file_name);

  slam_ros_msgs::msg::MapdataProgress getMapProgress();
  void load_ignore_regions_once();
 private:
  struct Subscriber {
    rclcpp::SubscriptionBase::SharedPtr subscriber;
    std::string topic;
  };

  void HandleSubmapQuery(
      const std::shared_ptr<slam_ros_msgs::srv::SubmapQuery::Request> request,
      std::shared_ptr<slam_ros_msgs::srv::SubmapQuery::Response> response);
  void HandleTrajectoryQuery(
      const std::shared_ptr<slam_ros_msgs::srv::TrajectoryQuery::Request>
          request,
      std::shared_ptr<slam_ros_msgs::srv::TrajectoryQuery::Response> response);
  void HandleStartTrajectory(
      const std::shared_ptr<slam_ros_msgs::srv::StartTrajectory::Request>
          request,
      std::shared_ptr<slam_ros_msgs::srv::StartTrajectory::Response> response);
  void HandleFinishTrajectory(
      const std::shared_ptr<slam_ros_msgs::srv::FinishTrajectory::Request>
          request,
      std::shared_ptr<slam_ros_msgs::srv::FinishTrajectory::Response> response);
  void HandleWriteState(
      const std::shared_ptr<slam_ros_msgs::srv::WriteState::Request> request,
      std::shared_ptr<slam_ros_msgs::srv::WriteState::Response> response);
  void HandleGetTrajectoryStates(
      const std::shared_ptr<slam_ros_msgs::srv::GetTrajectoryStates::Request>
          request,
      std::shared_ptr<slam_ros_msgs::srv::GetTrajectoryStates::Response>
          response);
  void HandleReadMetrics(
      const std::shared_ptr<slam_ros_msgs::srv::ReadMetrics::Request> request,
      std::shared_ptr<slam_ros_msgs::srv::ReadMetrics::Response> response);
  void HandleOnlyUseOdomPredictPoseFlag(
      const std::shared_ptr<
          slam_ros_msgs::srv::OnlyUseOdomPredictPose::Request>
          request,
      std::shared_ptr<
          slam_ros_msgs::srv::OnlyUseOdomPredictPose::Response>
          response);
  void HandleLogSlamData(
      const std::shared_ptr<slam_ros_msgs::srv::LogSlamData::Request> request,
      std::shared_ptr<slam_ros_msgs::srv::LogSlamData::Response> response);
  void HandleControlSLAM(
      const std::shared_ptr<slam_ros_msgs::srv::ControlSam::Request> request,
      std::shared_ptr<slam_ros_msgs::srv::ControlSam::Response> response);
  void HandlePgmToLocalizationMap(
      const std::shared_ptr<slam_ros_msgs::srv::PgmToLocalizationMap::Request>
          request,
      std::shared_ptr<slam_ros_msgs::srv::PgmToLocalizationMap::Response>
          response);
  void HandleLandMarkLists(
      const std::shared_ptr<slam_ros_msgs::srv::GetLandmarkLists::Request>
          request,
      std::shared_ptr<slam_ros_msgs::srv::GetLandmarkLists::Response> response);

  std::set<::slam::mapping::TrajectoryBuilderInterface::SensorId>
  ComputeExpectedSensorIds(const TrajectoryOptions& options) const;
  int AddTrajectory(const TrajectoryOptions& options);
  void LaunchSubscribers(const TrajectoryOptions& options, int trajectory_id);
  void PublishSubmapList();
  void AddSensorSamplers(int trajectory_id, const TrajectoryOptions& options);
  void PublishLocalTrajectoryData();
  void PublishTrajectoryNodeList();
  void PublishLandmarkPosesList();
  void PublishConstraintList();
  bool ValidateTrajectoryOptions(const TrajectoryOptions& options);
  bool ValidateTopicNames(const TrajectoryOptions& options);
  slam_ros_msgs::msg::StatusResponse FinishTrajectoryUnderLock(
      int trajectory_id);
  void MaybeWarnAboutTopicMismatch();

  bool SetOnlyUseOdomPredictPoseFlag(bool flag);
  void ResetSoftwareException();

  geometry_msgs::msg::PointStamped ToPointStampedPose(
      const Rigid3d& robot_pose, const rclcpp::Time& time);
  slam_ros_msgs::msg::PoseWithConfidence ToPoseWithConfidence(
      const Rigid3d& robot_pose, const rclcpp::Time& time);
  geometry_msgs::msg::PoseStamped ToPoseStamped(const Rigid3d& robot_pose,
                                                const rclcpp::Time& time);
  agv_msgs::msg::PoseWithConfidence ToTrackedPoseWithConfidence(
      const Rigid3d& robot_pose, const rclcpp::Time& time);

  void LogRobotPose();
  void TimedClearLog();
  void PublishSlamError();

  slam_ros_msgs::msg::StatusResponse TrajectoryStateToStatus(
      int trajectory_id,
      const std::set<::slam::mapping::PoseGraphInterface::TrajectoryState>&
          valid_states);
  void setSavemapProgress(slam_ros_msgs::msg::MapdataProgress& msg);
  void HandleAGVMode(const agv_msgs::msg::AGVMode::ConstPtr& msg);

  const NodeOptions node_options_;
  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;

  absl::Mutex mutex_;
  std::unique_ptr<slam_ros::metrics::FamilyFactory> metrics_registry_;
  MapBuilderBridge map_builder_bridge_;

  rclcpp::Subscription<agv_msgs::msg::AGVMode>::SharedPtr agv_mode_sub_;

  // ROS2 publishers
  rclcpp::Publisher<slam_ros_msgs::msg::SubmapList>::SharedPtr
      submap_list_publisher_;
  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr
      trajectory_node_list_publisher_;
  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr
      landmark_poses_list_publisher_;
  rclcpp::Publisher<slam_ros_msgs::msg::LandmarkListPose>::SharedPtr
      agv_landmark_poses_list_publisher_;
  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr
      constraint_list_publisher_;
  rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr robot_pose_pub_;
  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr
      robot_pose_with_confidence_pub_;
  rclcpp::Publisher<agv_msgs::msg::PoseWithConfidence>::SharedPtr
      pose_with_confidence_pub_;
  rclcpp::Publisher<agv_msgs::msg::ConfidenceLevel>::SharedPtr
      cur_confidence_pub_;
  rclcpp::Publisher<agv_msgs::msg::PoseWithConfidence>::SharedPtr
      agv_robot_status_pub_;
  rclcpp::Publisher<agv_msgs::msg::AGVEvent>::SharedPtr slam_event_pub_;
  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr
      scan_matched_point_cloud_publisher_;
  rclcpp::Publisher<sensor_msgs::msg::PointCloud>::SharedPtr
      robot_matched_point_cloud_publisher_;
  rclcpp::Publisher<slam_ros_msgs::msg::RobotStatus>::SharedPtr robot_status_;
  rclcpp::Publisher<slam_ros_msgs::msg::ReflectorLandmark>::SharedPtr
      reflector_landmark_pub_;
  rclcpp::Publisher<agv_msgs::msg::PoseWithConfidence>::SharedPtr fusion_laserpose_pub_;

  // ROS2 service servers
  std::vector<rclcpp::ServiceBase::SharedPtr> service_servers_;

  // ROS2 timers
  rclcpp::TimerBase::SharedPtr publish_local_trajectory_data_timer_;
  rclcpp::TimerBase::SharedPtr submap_list_timer_;
  rclcpp::TimerBase::SharedPtr trajectory_node_list_timer_;
  rclcpp::TimerBase::SharedPtr landmark_poses_list_timer_;
  rclcpp::TimerBase::SharedPtr constraint_list_timer_;
  rclcpp::TimerBase::SharedPtr wall_timers_;
  rclcpp::TimerBase::SharedPtr log_robot_pose_timer_;
  rclcpp::TimerBase::SharedPtr publish_slam_error_timer_;
  rclcpp::TimerBase::SharedPtr clear_log_timer_;

  uint32_t robot_pose_seq_ = 0;
  SlamType slam_type_ = SlamType::UNKNOWN;

  struct TrajectorySensorSamplers {
    TrajectorySensorSamplers(double rangefinder, double odometry,
                             double fixed_frame, double imu, double landmark,
                             double qr_code)
        : rangefinder_sampler(rangefinder),
          odometry_sampler(odometry),
          fixed_frame_pose_sampler(fixed_frame),
          imu_sampler(imu),
          landmark_sampler(landmark),
          qr_code_sampler(qr_code) {}

    ::slam::common::FixedRatioSampler rangefinder_sampler;
    ::slam::common::FixedRatioSampler odometry_sampler;
    ::slam::common::FixedRatioSampler fixed_frame_pose_sampler;
    ::slam::common::FixedRatioSampler imu_sampler;
    ::slam::common::FixedRatioSampler landmark_sampler;
    ::slam::common::FixedRatioSampler qr_code_sampler;
  };

  std::unordered_map<int, TrajectorySensorSamplers> sensor_samplers_;
  std::unordered_map<int, std::vector<Subscriber>> subscribers_;
  std::unordered_set<std::string> subscribed_topics_;
  std::unordered_set<int> trajectories_scheduled_for_finish_;

  bool is_laser_data_ok_ = true;
  bool is_slam_laser_data_ok_ = true;
  bool is_localization_ok_ = false;
  bool is_laser_ok_ = true;
  bool is_odom_ok_ = true;
  bool is_imu_ok_ = true;
  int is_laser_nav = 0;
  std::unique_ptr<SensorDataCache> sensor_data_cache_;
  bool log_sensor_data_;
  std::atomic<bool> stop_sensor_data_cache_thread_;
  std::unique_ptr<std::thread> sensor_data_cache_thread_;
  Rigid3d last_robot_pose_ = Rigid3d::Identity();
  Rigid3d robot_pose_now_ = Rigid3d::Identity();
  ::slam::common::Time last_pose_time_ = ::slam::common::Time::min();
  double laser_data_time_last_ = 0.0;
  double odometry_data_time_last_ = 0.0;
  double imu_data_time_last_ = 0.0;
  sensor_msgs::msg::LaserScan last_laser_msg_;
  agv_msgs::msg::OdometryLite last_odom_msg_;
  sensor_msgs::msg::Imu last_imu_msg_;
  Rigid3d T_map_odom_ = Rigid3d::Identity();
  TrajectoryOptions trajectory_options_;
  std::string current_localization_map_ = "NULL";
  std::string current_ZoneTag_ = "NULL";
  int current_trajectory_id_ = 0;
  bool saved_first_pose_ = false;
  geometry_msgs::msg::Transform laser_to_base_;
  const double distance_traveled_threshold_ = 0.1;
  const double angle_traveled_threshold_ = 0.1;
  bool is_scan_initialized_ = false;
  bool is_odom_initialized_ = false;
  bool is_imu_initialized_ = false;
  int32_t slam_cmd_ = 1;
  agv_msgs::msg::AGVEvent slam_event_msg;
  bool begin_start_device = true;
  slam_ros_msgs::msg::MapdataProgress save_map_progress_msg_;
  std::string current_save_map_id_ = "NULL";
  // Assuming ClearLog is a class you have defined elsewhere
  // std::unique_ptr<ClearLog> clear_log_;
  absl::Mutex pose_publish_mutex_;

  int current_agv_mode_ = -1;
  int last_current_agv_mode_ = -1;

  // 屏蔽区配置相关成员
  std::vector<std::vector<double>>
      ignore_regions_;    // 存储屏蔽区 [x_min, y_min, x_max, y_max]
  bool ignore_regions_loaded_ = false;  // 标记是否已加载屏蔽区
  // std::string ignore_region_config_path_ = "/home/vensin/map/66_region.json";
  // // 配置文件路径
  std::string ignore_region_config_path_ = "";   // 配置文件路径
  bool first_transform_obtained_ = false;  // 标志位
  tf2_ros::Buffer* tf_buffer_;  // 声明为指针类型
  geometry_msgs::msg::TransformStamped base_to_laser_transform_;  // 保存首次变换
  agv_msgs::msg::PoseWithConfidence last_PoseWithConfidence_msg_;
  geometry_msgs::msg::PointStamped last_robot_stamped_pose_;
  absl::Mutex data_mutex_;  
};

}  // namespace slam_ros

#endif  // SLAM_ROS_SLAM_ROS_NODE_H